/**
 * @file laser_node.cpp
 * @brief 
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-05-03
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */

#include <iostream>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

double yaw_base2laser = 0;
double coeffi_lf = 0.5;
double min_dist_angle = 0;
ros::Publisher result_pub;
double angle_range[] = {-60, 60};
double deg2rad = 3.1415926535 / 180.0f;

void scanCallback(const sensor_msgs::LaserScan::ConstPtr scan_msgs){
    // ROS_INFO("length: %d", scan_msgs->ranges.size());
    // ROS_INFO("min   : %f", scan_msgs->range_min);

    double angle_min = scan_msgs->angle_min + yaw_base2laser;
    double angle_max = scan_msgs->angle_max + yaw_base2laser;
    double angle_delta = scan_msgs->angle_increment;    

    double min_dist = INFINITY;
    static double last_min_dist = 0;
    static double last_min_dist_angle = 0;

    for(int i = 0; i < scan_msgs->ranges.size(); i++){
        double current_angle = angle_min + i * angle_delta;
        if(current_angle > angle_range[0] * deg2rad 
            && current_angle < angle_range[1] * deg2rad){
            if(scan_msgs->ranges[i] < min_dist){
                min_dist = scan_msgs->ranges[i];
                min_dist_angle = current_angle;
            }
        }
    }
    min_dist = coeffi_lf * last_min_dist + (1 - coeffi_lf) * min_dist;
    last_min_dist = min_dist;
    min_dist_angle = coeffi_lf * last_min_dist_angle + (1 - coeffi_lf) * min_dist_angle;
    last_min_dist_angle = min_dist_angle;
    
    // ROS_INFO("min_dist: %f", min_dist);
    // ROS_INFO("angle   : %f", min_dist_angle / deg2rad);

    geometry_msgs::Point p;
    p.z = min_dist;
    result_pub.publish(p);
}


int main(int argc, char** argv){
    ros::init(argc, argv, "laser_node");
    ros::NodeHandle n;

    ros::Subscriber laser_scan_sub = n.subscribe("/scan", 1, scanCallback);
    result_pub = n.advertise<geometry_msgs::Point>("/laser_node/min_dist",10);

    ros::Rate loop_rate(10);

    ros::NodeHandle private_nh("~");      
    private_nh.param("angle_range_min", angle_range[0], -60.0);
    private_nh.param("angle_range_max", angle_range[1], -60.0);
    ROS_INFO("angle_range_min: %f", angle_range[0]);
    ROS_INFO("angle_range_max: %f", angle_range[1]);

    // 监听tf
    tf::TransformListener listener;
    tf::StampedTransform transform;
    try
    {
        // 监听从laser到base_link的转换
        listener.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/base_link", "/laser", ros::Time(0), transform);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s", ex.what());
        ros::Duration(1.0).sleep();
    }
    
    yaw_base2laser = tf::getYaw(transform.getRotation());
    ROS_INFO("yaw_base2laser: %f", yaw_base2laser);

    while(ros::ok()){
        ros::spinOnce();
        loop_rate.sleep();
    }


}
